/*
	Copyright 2008 Jay Elliott

	This file is part of libemath.

	libemath is free software: you can redistribute it and/or modify
	it under the terms of the GNU General Public License as published by
	the Free Software Foundation, either version 3 of the License, or
	(at your option) any later version.

	libemath is distributed in the hope that it will be useful,
	but WITHOUT ANY WARRANTY; without even the implied warranty of
	MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
	GNU General Public License for more details.

	You should have received a copy of the GNU General Public License
	along with libemath.  If not, see <http://www.gnu.org/licenses/>.
*/

#include "cQuaternion.h"




emath::cQuaternion::cQuaternion(float angle, emath::cPoint3d axis)
{
	axis.normalize();

	float HalfAngle = Deg2Rad(angle / 2.0f);


	w = cos(HalfAngle);
	x = axis[0] * sin(HalfAngle);
	y = axis[1] * sin(HalfAngle);
	z = axis[2] * sin(HalfAngle);
}
emath::cQuaternion emath::cQuaternion::operator*(const emath::cQuaternion& other) const
{
	cQuaternion result;

	cPoint3d MyVec(x, y, z);
	cPoint3d OtherVec(other.x, other.y, other.z);
	cPoint3d ResultVec;

	result.w = w * other.w - MyVec.dot(OtherVec);
	ResultVec = w * OtherVec + other.w * MyVec + MyVec.cross(OtherVec);

	result.x = ResultVec[0];
	result.y = ResultVec[1];
	result.z = ResultVec[2];

	return result;
}

void emath::cQuaternion::GetMatrix(float mat[16]) const
{
	//TODO: this could be optimized a bit by doing stuff like "2.0f * (y * y - z * z)" instead of "2.0f * y * y - 2.0f * z * z"
	mat[0] = 1.0f - 2.0f * y * y - 2.0f * z * z;
	mat[1] = 2.0f * x * y + 2.0f * w * z;
	mat[2] = 2.0f * x * z - 2.0f * w * y;
	mat[3] = 0.0f;

	mat[4] = 2.0f * x * y - 2.0f * w * z;
	mat[5] = 1.0f - 2.0f * x * x - 2.0f * z * z;
	mat[6] = 2.0f * y * z + 2.0f * w * x;
	mat[7] = 0.0f;

	mat[8] = 2.0f * x * z + 2.0f * w * y;
	mat[9] = 2.0f * y * z - 2.0f * w * x;
	mat[10] = 1.0f - 2.0f * x * x - 2.0f * y * y;
	mat[11] = 0.0f;

	mat[12] = 0.0f;
	mat[13] = 0.0f;
	mat[14] = 0.0f;
	mat[15] = 1.0f;
}

void emath::cQuaternion::GetAxisAngle(float *angle, emath::cPoint3d *axis)
{
	normalize();

	*angle = Rad2Deg(acos(w) * 2);

	float AngleSin = sqrt(1.0f - w * w);

	if (fabsf(AngleSin) < 0.0005f)
		AngleSin = -1.0f;

	(*axis)[0] = x / AngleSin;
	(*axis)[1] = y / AngleSin;
	(*axis)[2] = z / AngleSin;
}



emath::cQuaternion emath::cQuaternion::inverse(void) const
{
	fprintf(stderr, "Warning: \'emath::cQuaternion emath::cQuaternion::inverse(void)\' const is deprecated.  Use \'emath::cQuaternion wmath::cQuaternion::conjugate(void)\' const instead.\n"); 
	return conjugate();
}
